import Unitree_Python_sdk
unitree_robot = Unitree_Python_sdk.Unitree_Robot_High()

import pygame
import time
import sys
sys.path.append('/home/a/unitree_sdk/unitree_legged_sdk-3.2/third_party')
import xbox.xbox_controller as xbox_controller


def control():
    pygame.init()
    controller = xbox_controller.Controller()
    playing = False
    while True:
        time.sleep(0.002)
        # event handling
        for event in pygame.event.get():
            if event.type == pygame.QUIT:
                pygame.quit()
                break

            if event.type == pygame.JOYBUTTONDOWN:
                # handle events for all controllers
                if not playing:
                    if event.button == xbox_controller.START: playing = True
                    else: continue
                else:
                    if event.button == xbox_controller.BACK: playing = False

                if event.button == xbox_controller.LEFT_BUMP:
                    print('LB down')

                if event.button == xbox_controller.RIGHT_BUMP:
                    print('RB down')

            elif event.type == pygame.JOYBUTTONUP:
                if not playing: continue
                if event.button == xbox_controller.LEFT_BUMP:
                    print('LB up')

                if event.button == xbox_controller.RIGHT_BUMP:
                    print('RB up')

            elif event.type == pygame.JOYHATMOTION:
                if not playing: continue
                pad_up, pad_right, pad_down, pad_left = controller.get_pad()
                pass

        if playing:
            # handle joysticks
            left_x, left_y   = controller.get_left_stick()
            right_x, right_y = controller.get_right_stick()
            forwardSpeed = left_y * 0.5
            sideSpeed = left_x * 0.5
            rotateSpeed = right_x *0.5
            unitree_robot.robot_walking(forwardSpeed=forwardSpeed, sideSpeed=sideSpeed, rotateSpeed=rotateSpeed, bodyHeight=0.0, footRaiseHeight=0.1)
        else:
            unitree_robot.robot_walking(forwardSpeed=0.0, sideSpeed=0.0, rotateSpeed=0.0, bodyHeight=0.0, footRaiseHeight=0.1)

        # clock.tick(FPS)


control()
